﻿using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System;

public partial class InterpolationCalculate  {

    public int interpolation = 1;

    #region Newton
    /// <summary>
    /// 递归计算商差
    /// </summary>
    /// <param name="points"></param>
    /// <returns></returns>
    static float DifferenceQuotient(Vector2[] points)
    {
        if (points == null || points.Length <= 0)
            return 0;

        if (points.Length == 1)
            return points[0].y;

        if(points.Length == 2)
        {
            if (points[0].x == points[1].x)
                return 0;
            else
                return (points[0].y - points[1].y) / (points[0].x - points[1].x);
        }

        Vector2[] f0 = new Vector2[points.Length - 1];
        Vector2[] f1 = new Vector2[points.Length - 1];

        for (int i = 0; i < f0.Length; i++)
        {
            f0[i] = points[i];
            f1[i] = points[i + 1];
        }
            float ret = 0;
        if (points[0].x != points[points.Length - 1].x)
            ret = (DifferenceQuotient(f0) - DifferenceQuotient(f1)) / (points[0].x - points[points.Length - 1].x);
        
        return ret;
    }

    /// <summary>
    /// 牛顿插值法，是一个关于x的n次多项式
    /// </summary>
    /// <param name="points"></param>
    /// <returns></returns>
    public static Func<float, float> NewtonInterpolation(Vector2[] points)
    {
       Func<float, float> ret = (x) =>
       {
           float sum = points[0].y;

           for (int i = 1; i < points.Length; i++)
           {
               Vector2[] p = new Vector2[i + 1];
               for (int j=0;j< p.Length;j++)
               {
                   p[j] = points[j];
               }

               float t = 1;

               for (int j = 0; j < i; j++)
               {
                   t *= (x - points[j].x);
               }
             //  Debug.Log("t:" + t + "   DifferenceQuotient(p) :"+ DifferenceQuotient(p));
               sum += DifferenceQuotient(p) * t;
           }
           return sum;
       };


        return ret;
    }

    #endregion

    #region Spline
    //二次样条线插值

    public static Func<float, float> QuadraticSplineInterpolation(Vector2[] points)
    {
        if (points == null || points.Length <= 0)
        {
            throw new ArgumentException("点输入不合法！");
        }

        var list = new List<Vector2>(points);
        list.Sort((a, b) => { return a.x.CompareTo(b.x); });
        points = list.ToArray();

        if (points.Length == 1)
            return (x)=> { return x; };

        if (points.Length == 2)
        {
            return LineFunction(points[0], points[1]);
        }
        
        Func<float, float> res = (x) =>
        {
            float y = 0;

            int index = points.Length - 1;
            for(int i = 0; i < points.Length; i++)
            {
                if(x >= points[i].x)
                {
                    index = i;
                }
            }
            Func<float, float> curFunc = null;
            if (index >= 1)
            {
                if (index + 1< points.Length)
                    curFunc = QuadraticFunction(points[index - 1], points[index], points[index + 1]);
                else
                    curFunc = QuadraticFunction(points[points.Length - 3], points[points.Length - 2], points[points.Length - 1]);
            }
            else
            {
                curFunc = QuadraticFunction(points[0], points[1], points[2]);
            }
            Func<float, float> nextFunc = null;
            if (index + 2 < points.Length)
            {
                if (index + 1 < points.Length)
                    nextFunc = QuadraticFunction(points[index], points[index+1], points[index + 2]);
            }

            float curY = curFunc(x);
            if(nextFunc != null)
            {
                float nextY = nextFunc(x);

                float curLength = index+1< points.Length? points[index+1].x - points[index].x: 1;
                float v = (x - points[index].x)/curLength;
                y = Mathf.Lerp(curY, nextY, v);
            }
            else
            {
                y = curY;
            }

            return y;
        };

        return res;
    }

    public static Func<float, float> LineFunction(Vector2 p0, Vector2 p1)
    {
        return (x) =>
        {
            float k = (p1.y - p0.y) / (p1.x - p0.x);
            float b = p0.y - k * p0.x;
            return k * x + b;
        };
     }

    /// <summary>
    /// 三点确定一条抛物线函数
    /// </summary>
    /// <returns></returns>
    public static Func<float, float> QuadraticFunction(Vector2 p0, Vector2 p1, Vector2 p2)
    {
        if(p0.x == p1.x && p1.x == p2.x)
        {
            throw new ArgumentException("点输入不合法！");
        }

        if(p0.x == p1.x)
        {
            return LineFunction(p1, p2);
        }
        if (p0.x == p2.x)
        {
            return LineFunction(p1, p2);
        }
        if (p1.x == p2.x)
        {
            return LineFunction(p0, p1);
        }

        Func<float, float> res = (x) =>
        {
            float x0 = p0.x;
            float y0 = p0.y;
            float x1 = p1.x;
            float y1 = p1.y;
            float x2 = p2.x;
            float y2 = p2.y;

            ///应用计算出来的公式
            float temp = -x0*x2*x2 + x1*x2*x2 - x2*x1*x1 + x2*x0*x0 + x0*x1*x1 - x1*x0*x0;

            float b = (temp == 0)? 0 : (y0*(x1*x1-x2*x2) + y1*(x2*x2 - x0*x0) + y2 * (x0*x0 - x1*x1)) / temp;

            float c = ((y0 - b * x0) * x1 * x1 - (y1 - b * x1) * x0 * x0) / (x1 * x1 - x0 * x0);

            float a = 0;

            if (x0 != 0)
                a = (y0 - b * x0 - c) / (x0 * x0);
            else if (x1 != 0)
                a = (y1 - b * x1 - c) / (x1 * x1);
            else if (x2 != 0)
                a = (y2 - b * x2 - c) / (x2 * x2);
            else
                a = 0;

            float y = a * x * x + b * x + c;

            aT = a;
            bT = b;
            cT = c;
            return y;
        };

        return res;
    }

    public static float aT;
    public static float bT;
    public static float cT;

    public static float k1T;
    public static float k2T;
    public static float angleT;


    public static Curve QuadraticCurveInterpolation(Vector2[] points)
    {
        if (points == null || points.Length <= 0)
        {
            throw new ArgumentException("点输入不合法！");
        }
        
        if (points.Length == 1)
            return new Curve((t) => { return points[0]; });

        if (points.Length == 2)
        {
            return LineCurve(points[0], points[1]);
        }

        Func<float, Vector2> res = (t) =>
        {
            Vector2 p = Vector2.zero;

            int index = points.Length - 1;
            float length = 0;

            float totalLength = CalculateLineXLength(points);
            float curLength = totalLength * t;
            List<Vector2> curPoints = new List<Vector2>();

            int curCurveBeginIndex = 0;
            int curCurveEndIndex = 0;
            for (int i = 0; i < points.Length; i++)
            {
                curPoints.Add(points[i]);
                length = CalculateLineXLength(curPoints.ToArray());

                if (length  > curLength)
                {
                    index = i - 1< 0 ? 0 : i - 1; 
                    break;
                }
            }
          
            Curve curCurve = null;
            if (index >= 1)
            {
                if (index + 1 < points.Length)
                {
                    curCurveBeginIndex = index - 1;
                    curCurveEndIndex = index + 1;
                    curCurve = QuadraticCurve(points[index - 1], points[index], points[index + 1]);
                }
                else
                {
                    curCurveBeginIndex = points.Length - 3;
                    curCurveEndIndex = points.Length - 1;
                    curCurve = QuadraticCurve(points[points.Length - 3], points[points.Length - 2], points[points.Length - 1]);
                }
            }
            else
            {
                curCurveBeginIndex = 0;
                curCurveEndIndex = 2;
                curCurve = QuadraticCurve(points[0], points[1], points[2]);
            }
            Curve nextCurve = null;
            if (index >0 && index + 2 < points.Length)
            {
                 nextCurve = QuadraticCurve(points[index], points[index + 1], points[index + 2]);
            }

            float curCurveXLength = CalculateSubLineXLength(points, curCurveBeginIndex, curCurveEndIndex);
            float curT = (curLength - CalculateSubLineXLength(points, 0, curCurveBeginIndex)) / curCurveXLength;

           // Debug.Log("curT:" + curT + "   t：" + t);

            Vector2 curP = curCurve.func(curT);

            if (nextCurve != null)
            {
                float nextCurveXLength = CalculateSubLineXLength(points, index, index + 2);
                float nextT = (curLength - CalculateSubLineXLength(points, 0, index)) / nextCurveXLength;
                Vector2 nextP = nextCurve.func(nextT);

                float curDegreeLength = CalculateSubLineXLength(points, 0, index);
                float nextDegreeLength = CalculateSubLineXLength(points, 0, index + 1);

                float v = (curLength - curDegreeLength) / (nextDegreeLength - curDegreeLength);

                p = curP + (nextP - curP) * v;

                //testShowBlend[(int)(t * 10000)] = v;
                
                //Debug.Log("blend t:" + t + "  v:" + v + "  curLength:" + curLength + "  index:"+ index + "  curDegreeLength:" + curDegreeLength +"  totalLength:" + totalLength);
            }
            else
            {
                p = curP;
            }
            return p;
        };

        return new Curve(res);
    }

    public static Dictionary<int, float> testShowBlend = new Dictionary<int, float>();
    

    static float CalculateSubLineXLength(Vector2[] points,int fromIndex,int toIndex)
    {
        if (fromIndex > toIndex) return 0;
        if (fromIndex < 0)
            fromIndex = 0;
        if (toIndex >= points.Length)
            toIndex = points.Length - 1;


        int length = toIndex - fromIndex + 1;
        Vector2[] p = new Vector2[length];
        int i = 0;
        for (int index = fromIndex; index <= toIndex;index++)
        {
            p[i] = points[index];
            i++;
        }
        return CalculateLineXLength(p);
    }

    static float CalculateLineXLength(Vector2[] points)
    {
        if (points == null) return 0;

        float totalLength = 0;
        for (int i = 0; i < points.Length - 1; i++)
        {
            Vector2 curV = points[i + 1] - points[i];
            totalLength += Mathf.Abs(curV.x);
        }
        return totalLength;
    }

    public static Vector3 p0Pos;
    public static Vector3 p1Pos;
    public static Vector3 p2Pos;
    public static Vector3 p0MPos;
    public static Vector3 p2MPos;

    public static Curve LineCurve(Vector2 p0, Vector2 p1)
    {
        return new Curve((t) => {
            float x = Mathf.Lerp(p0.x, p1.x, t);
            return new Vector2(x, LineFunction(p1, p1)(x));
        });
    }


    /// <summary>
    /// 三点确定一条抛物线线段
    /// </summary>
    /// <returns></returns>
    public static Curve QuadraticCurve(Vector2 p0, Vector2 p1, Vector2 p2)
    {
        if (p0 == p1 && p0 == p2)
        {
            throw new ArgumentException("点输入不合法！");
        }

        if (p0 == p1)
            return LineCurve(p0, p2);

        if (p0 == p2)
            return LineCurve(p0, p1);

        p0Pos = p0;
        p1Pos = p1;
        p2Pos = p2;

        Vector2 vector1 = p0 - p1;
        Vector2 vector2 = p2 - p1;

        float rad1 = Mathf.Atan2(vector1.y, vector1.x);
        float rad2 = Mathf.Atan2(vector2.y, vector2.x);

        float radM = (rad1 + rad2) / 2;
        
        float downRad = 3 * Mathf.PI / 2;

        float radian = radM< downRad ? downRad - radM: -(radM - downRad);

        k1T = Mathf.Rad2Deg * rad1;
        k2T = Mathf.Rad2Deg * rad2;
        angleT = Mathf.Rad2Deg * radian;
        //  Debug.Log("k1:" + k1+ "  k2:"+ k2 + "  radian :" + radian);

        ///先旋转为二次函数的三个点,求出二次函数
        Vector2 p0M = RotatePoint(p0, p1, Vector3.forward, radian);
        Vector2 p2M = RotatePoint(p2, p1, Vector3.forward, radian);
        p0MPos = p0M;
        p2MPos = p2M;
        var qf = QuadraticFunction(p0M, p1, p2M);

        Func<float, Vector2> qfCurveFunc = (tM) =>
        {
            //可小于0
            float x = p0M.x + (p2M.x - p0M.x) * tM;
            //float x = Mathf.Lerp(p0M.x, p2M.x, tM);
            float y = qf(x);
            Vector2 p = new Vector2(x,y);
            return p;
        };

        Curve qfCurve = new Curve(qfCurveFunc);
        
        //再旋转为经过原三个点的线段
        Curve res = CurveRotate(qfCurve,p1,Vector3.forward, -radian);

        res.beginPoint = p0;
        res.endPoint = p2;

        return res;
    }

    /// <summary>
    /// 点旋转
    /// </summary>
    /// <param name="src"></param>
    /// <param name="origin"></param>
    /// <param name="axis"></param>
    /// <param name="radian "></param>
    /// <returns></returns>
    public static Vector2 RotatePoint(Vector2 src,Vector2 origin, Vector3 axis,float radian)
    {
        Vector2 srcV = src - origin;
        float l = srcV.magnitude;// Mathf.Sqrt(srcV.x * srcV.x + srcV.y * srcV.y);

        float a1 = srcV.x == 0? Mathf.PI/2 : Mathf.Atan2(srcV.y,srcV.x);
        float b = a1 + radian;
        
        float x = l * Mathf.Cos(b);
        float y = l * Mathf.Sin(b);
        return origin + new Vector2(x,y);
    }

    /// <summary>
    /// 曲线旋转
    /// </summary>
    /// <param name="curve"></param>
    /// <param name="origin"></param>
    /// <param name="axis"></param>
    /// <param name="radian "></param>
    /// <returns></returns>
    public static Curve CurveRotate(Curve curve,Vector2 origin,Vector2 axis,float radian)
    {
        Func<float, Vector2> f = (t) => 
        {
            if (curve.func == null)
                return Vector2.zero;

            return curve.func(t);
        };

        Curve res = new Curve();
        res.func = (t) =>
        {
            var p = f(t);
            p = RotatePoint(p, origin, axis, radian);
            return p;
        };

        return res;
    }

    #endregion
}

public class Curve
{
    public Vector2 beginPoint;
    public Vector2 endPoint;
    public Func<float, Vector2> func;
    
    public Curve()
    {

    }

    public Curve(Func<float, Vector2> _func)
    {
        func = _func;
    }

    public float[] GetPoint(float x,out float[] t)
    {
        t = new float[1];


        float[] res = new float[1];
        return res;
    }

    public Vector2 GetPoint(float _t)
    {
       // _t = Mathf.Clamp01(_t);

        if (func != null)
            return func(_t);

        return Vector2.zero;
    }
}
